#include "kal_uwb_flow.hpp"

void KALUWBFLOW::KalUwbFlow_Init()
{
	Update = false;
	dt = 0.005f;
//	dt = 0.001f;

	xQueuePeek(queueUWB, &uwbPos, 0);					//从队列中获取UWB数据
	xQueuePeek(queueFlow,&flow, 0);
	xQueuePeek(queueHeight, &height, 0);

	//参数初始化
	F <<    1.0f,0.0f,0.0f,  dt,0.0f,0.0f,0.0f,0.0f,0.0f,
			0.0f,1.0f,0.0f,0.0f,  dt,0.0f,0.0f,0.0f,0.0f,
			0.0f,0.0f,1.0f,0.0f,0.0f,  dt,0.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f,1.0f,0.0f,0.0f, -dt,0.0f,0.0f,
			0.0f,0.0f,0.0f,0.0f,1.0f,0.0f,0.0f, -dt,0.0f,
			0.0f,0.0f,0.0f,0.0f,0.0f,1.0f,0.0f,0.0f, -dt,
			0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,1.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,1.0f,0.0f,
			0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,1.0f;

	B <<    0.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f,
			dt  ,0.0f,0.0f,
			0.0f,dt  ,0.0f,
			0.0f,0.0f,dt  ,
			0.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f;

	G <<    0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,
			dt  ,0.0f,0.0f,0.0f,0.0f,0.0f,
			0.0f,dt  ,0.0f,0.0f,0.0f,0.0f,
			0.0f,0.0f,dt  ,0.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f,dt  ,0.0f,0.0f,
			0.0f,0.0f,0.0f,0.0f,dt  ,0.0f,
			0.0f,0.0f,0.0f,0.0f,0.0f,dt  ;

	H <<    1.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,
			0.0f,1.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,
			0.0f,0.0f,1.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f,1.0f,0.0f,0.0f,0.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f,0.0f,1.0f,0.0f,0.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f,0.0f,0.0f,1.0f,0.0f,0.0f,0.0f;

//	Q <<    1.0f,0.0f,0.0f,0.0f,0.0f,0.0f,
//			0.0f,1.0f,0.0f,0.0f,0.0f,0.0f,
//			0.0f,0.0f,1.0f,0.0f,0.0f,0.0f,
//			0.0f,0.0f,0.0f,0.001f,0.0f,0.0f,
//			0.0f,0.0f,0.0f,0.0f,0.001f,0.0f,
//			0.0f,0.0f,0.0f,0.0f,0.0f,0.001f;

//	Q <<	1.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,
//			0.0f,1.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,
//			0.0f,0.0f,1.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,
//			0.0f,0.0f,0.0f,0.01f,0.0f,0.0f,0.0f,0.0f,0.0f,
//			0.0f,0.0f,0.0f,0.0f,0.01f,0.0f,0.0f,0.0f,0.0f,
//			0.0f,0.0f,0.0f,0.0f,0.0f,0.01f,0.0f,0.0f,0.0f,
//			0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.001f,0.0f,0.0f,
//			0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.001f,0.0f,
//			0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.001f;
	Q <<	0.1f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,
			0.0f,0.1f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,
			0.0f,0.0f,0.1f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f,0.1f,0.0f,0.0f,0.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f,0.0f,0.1f,0.0f,0.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f,0.0f,0.0f,0.1f,0.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.001f,0.0f,0.0f,
			0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.001f,0.0f,
			0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.001f;

//	R <<    0.20f,0.0f,0.0f,0.0f,0.0f,0.0f,
//			0.0f,0.20f,0.0f,0.0f,0.0f,0.0f,
//			0.0f,0.0f,0.50f,0.0f,0.0f,0.0f,
//			0.0f,0.0f,0.0f,0.001f,0.0f,0.0f,
//			0.0f,0.0f,0.0f,0.0f,0.001f,0.0f,
//			0.0f,0.0f,0.0f,0.0f,0.0f,0.001f,
	R <<    1.4f,0.0f,0.0f,0.0f,0.0f,0.0f,
			0.0f,1.4f,0.0f,0.0f,0.0f,0.0f,
			0.0f,0.0f,2.0f,0.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f,0.001f,0.0f,0.0f,
			0.0f,0.0f,0.0f,0.0f,0.001f,0.0f,
			0.0f,0.0f,0.0f,0.0f,0.0f,0.001f,


	//初始状态，协方差矩阵

	X_k << uwbPos.uwbPos[0],uwbPos.uwbPos[1],uwbPos.uwbPos[2],flow.flowy,flow.flowx,height.speedZ,0.0f,0.0f,0.0f;

	P_k = 1e-4f*MatrixXf::Identity(9, 9);

	accUWB << 0.0f, 0.0f, 0.0f;
}

void KALUWBFLOW::KalUwbFlow_Pred()
{
	xQueuePeek(queueAccDat, &acc, 0);					//从队列中获取加速度计数据
	xQueuePeek(queueAhrsEuler, &ahrsEuler, 0);
//	xQueuePeek(queueControlMsg, &control_msg, 0);

//	yaw = control_msg.SetAng[2];
	yaw = ahrsEuler.Ang[2];

	Rz << cosf(yaw),-sinf(yaw),0.0f,
		  sinf(yaw), cosf(yaw),0.0f,
		  0.0f,	     0.0f,	   1.0f;

	a_k << acc.acc[0],acc.acc[1],acc.acc[2];			//机体坐标系下的加速度测量值

	//当前旋转矩阵
	q.w() = ahrsEuler.q[0];
	q.x() = ahrsEuler.q[1];
	q.y() = ahrsEuler.q[2];
	q.z() = ahrsEuler.q[3];
	q.normalize();
	Cb2n = q.toRotationMatrix();

	accUWBTemp += Rz * Cb2n * a_k;

//	u_k << accUWB[0],accUWB[1],accUWB[2]+9.788f;
//
//	//Predict state
//	X_p = F * X_p + B * u_k;
//
//	//Predict covariance matrix
////	P_p = F * P_k * F.transpose() + G * Q * G.transpose();
//	P_p = F * P_p * F.transpose() + Q;

//	kal_uwb_pos_vel.Pos[0] = X_p[0];
//	kal_uwb_pos_vel.Pos[1] = X_p[1];
//	kal_uwb_pos_vel.Pos[2] = X_p[2];
////	kal_uwb_pos_vel.Pos[2] = height.height;
//
//	kal_uwb_pos_vel.Vel[0] = X_p[3];
//	kal_uwb_pos_vel.Vel[1] = X_p[4];
//	kal_uwb_pos_vel.Vel[2] = X_p[5];
////	kal_uwb_pos_vel.Vel[2] = height.speedZ;
//
//	xQueueOverwrite(queueKalUwbPosVel,&kal_uwb_pos_vel);
}

void KALUWBFLOW::KalUwbFlow_Update()
{
	startTimerLast = startTimer;
	getTimer_us(&startTimer);
	cycleTime_us = startTimer - startTimerLast;

	xQueuePeek(queueUWB, &uwbPos, 0);					//从队列中获取UWB数据
	xQueuePeek(queueFlow,&flow, 0);
	xQueuePeek(queueHeight, &height, 0);

	accUWB = accUWBTemp / 5.0;
	accUWBTemp.setZero();

	u_k << accUWB[0],accUWB[1],accUWB[2]+9.788f;

	y << uwbPos.uwbPos[0],uwbPos.uwbPos[1],uwbPos.uwbPos[2],flow.flowy,flow.flowx,height.speedZ;

	//KF迭代过程

	//Predict state
	X_p = F * X_k + B * u_k;

	//Predict covariance matrix
//	P_p = F * P_k * F.transpose() + G * Q * G.transpose();
	P_p = F * P_k * F.transpose() + Q;

	//Computer Kalman Gain
	K = P_p * H.transpose() * (H * P_p * H.transpose() + R).inverse();

	//Update state
	X_k = X_p + K * (y - H * X_p);

	//Update covariance matrix
	P_k = (MatrixXf::Identity(9, 9) - K * H) * P_p;

//	X_p = X_k;
//	P_p = P_k;

	kal_uwb_pos_vel.Pos[0] = X_k[0];
	kal_uwb_pos_vel.Pos[1] = X_k[1];
	kal_uwb_pos_vel.Pos[2] = X_k[2];
//	kal_uwb_pos_vel.Pos[2] = height.height;

	kal_uwb_pos_vel.Vel[0] = X_k[3];
	kal_uwb_pos_vel.Vel[1] = X_k[4];
	kal_uwb_pos_vel.Vel[2] = X_k[5];
//	kal_uwb_pos_vel.Vel[2] = height.speedZ;


	xQueueOverwrite(queueKalUwbPosVel,&kal_uwb_pos_vel);

	getTimer_us(&stopTimer);

	executionTime_us = stopTimer - startTimer;
}

KALUWBFLOW KALUWBFLOW;

extern "C" void kalUwb_pred(void *argument)
{
	osDelay(3000);
	for(;;)
	{
		osSemaphoreAcquire(semKalUwbPred,0xffffffff);
		KALUWBFLOW.KalUwbFlow_Pred();
	}
}

extern "C" void kalUwb_main(void *argument)
{
	osDelay(2000);
	KALUWBFLOW.KalUwbFlow_Init();
	osDelay(200);

	uint32_t cnt = 0;

	for(;;)
	{
		osSemaphoreAcquire(semKalUwb,0xffffffff);

		KALUWBFLOW.KalUwbFlow_Pred();
		if (cnt % 5 == 4)
		{
			KALUWBFLOW.KalUwbFlow_Update();
		}
		cnt++;
	}
}
